#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <string.h>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

#include "draw_trajectory.h"

using namespace std;
using namespace Sophus;
using namespace Eigen;

const string es_trajectory_file = "../estimated.txt";
const string gr_trajectory_file = "../groundtruth.txt";
const string trajectory_file = "../trajectory.txt";

int main(int argc, char *argv[])
{
    std::cout << argc << endl;

    double rmse;
    
    if(argc == 2){
        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses;

        ifstream fin;
        fin.open(argv[1]);
        int id = 0;

        while (!fin.eof())
        {
            double time, tx, ty, tz, qx, qy, qz, qw;

            fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

            Eigen::Quaterniond q(qw, qx, qy, qz);
            q.normalize();

            Sophus::SE3d p1(q, Eigen::Vector3d(tx, ty, tz));
            poses.push_back(p1);
        }

        DrawTrajectory(poses);
    }
    else{
        ifstream fin_es(es_trajectory_file);
        ifstream fin_gr(gr_trajectory_file);

        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_es;
        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_gr;


        while (!fin_es.eof() && !fin_gr.eof())
        {
            double time, tx, ty, tz, qx, qy, qz, qw;
            fin_es >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

            Eigen::Quaterniond q(qw, qx, qy, qz);
            q.normalize();
            Sophus::SE3d p_es(q, Eigen::Vector3d(tx, ty, tz));
            poses_es.push_back(p_es);

            fin_gr >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

            Eigen::Quaterniond qq(qw, qx, qy, qz);
            qq.normalize();
            Sophus::SE3d p_gr(qq, Eigen::Vector3d(tx, ty, tz));
            poses_gr.push_back(p_gr);

            double error = (p_gr.inverse() * p_es).log().norm();
            rmse += error * error;
        }

        rmse = rmse / double(poses_es.size());
        rmse = sqrt(rmse);
        cout << "RMSE = " << rmse << endl;

        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses;
        //... vec1,vec2赋值
        poses.insert(poses.end(), poses_es.begin(), poses_es.end());
        poses.insert(poses.end(), poses_gr.begin(), poses_gr.end());

        DrawTrajectory(poses);
    }


    return 0;
}
